/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * EEPid is a functions collection to compute pid regulation.
 * 
 * = Set the robot pid parameters tutorial = 
 * == I. Ramp tuning ==
 * Lets start by adjusting the acceleration ramp.
 * For now there is 2 ramps to tune : 
 *  * Distance regulator acceleration ramp
 *  * Angle regulator acceleration ramp
 *
 * === 1. Distance acceleration ramp tuning ===
 * Disable the distance regulator acceleration ramp.
 * Send a step command and log real motor speed and effective motor speed.
 * 
 *   ODOLocomotion loco( 0.12f, 0.15f, 1000 );
 *   loco.setMotors( myLeftMotor, myRightMotor );
 *   loco.setDistanceRampEnable( false );
 *   
 *   Then set at target in axis X only
 *   ODOOdometry::= fieldPosition target;
 *   target.x = 0.5f; // 50 cm            
 *   target.y = 0.f;            
 *   target.a = 0.f;            
 *   target.d = 0.f;            
 *   loco.setTarget( target );
 *
 * === 2. Angle acceleration ramp tuning ===
 * Follow the same procedure but with angle regulator acceleration ramp disabled.
 *
 * == II. set PID parameters ==
 * === 1. ziegler_nichols ===
 * Lets exprim the pid equation in this term :
 *    u(t) = Kc * ( e(t) + ( 1/Ti ) * sum( e(t), t ) + Td * d(e(t))/dt )<br>
 * So in our formalism : 
 *    
 *    P = Kc</b></li>
 *    I = Kc/Ti</b></li>
 *    D = Kc*Td</b></li>
 *    
 * This is the Ziegler-Nichol methodology : 
 * 
 *  * Start with a low value of Kc.
 *  * Perturb the system -> oscillation of the output.
 *  * Increase Kc until sustained oscillation is observed in the system output.
 *  * KuIncrease Kc until sustained oscillation is observed in the system output.
 * 
 **/
package org.hh.locodo;


public class PID {
	
  /** Proportional coefficient of the pid regulator */
  public volatile double P;

  /** Integral coefficient of the pid regulator */
  public volatile double I;
  
  /** Derivative coefficient of the pid regulator */
  public volatile double D;
  
  /** limitation value of the integral */
  public volatile double iLimit;
  
  
  /** Last value of pid input */
  private double last_X;
  
  /** Last value of pid input Derivative */
  private double last_dX;
  
  /** Last value of pid input integral */
  private double last_iX;

  
  private double out;
  
  
  public PID(){
  	P = 1.;
  	I = 0.;
  	D = 0.;
  	this.iLimit = 0.;
  	this.last_X = 0.;
  	this.last_dX = 0.;
  	this.last_iX = 0.;
  	this.out = 0.;
  }

  public PID(double p, double i, double d, double iLimit) {
		P = p;
		I = i;
		D = d;
		this.iLimit = iLimit;
		this.last_X = 0.;
		this.last_dX = 0.;
		this.last_iX = 0.;
		this.out = 0.;
	}

	/**
   * computePID : compute new pid step according to the duration between two call.
   */

  public double update      ( ){
    out = P * last_X + I * last_iX + D * last_dX;
    if( out > 1.f)  out = 1.f;
    if( out < -1.f) out = -1.f;
    return out;
  }

  /**
   * add a new value to the filter. Needs to be call often and regularly
   */
  public void setCurrValue ( double X, int us ) {
    double DX = X - last_X;
    double dX = DX;// / (float)us;
    double iX = (last_iX + dX);// * us;


    last_X = X;
    last_dX = dX;
    last_iX += iX;
    if( last_iX > iLimit)
      last_iX = iLimit;
    if( last_iX < -iLimit)
      last_iX = -iLimit;

	}
}
